function [C0] = dcmnormortho_order(CHat, order) %#codegen
%DCMNORMORTHO_ORDER 对方向余弦矩阵进行规范正交化，按级数解方法
%
% Tips:
% # 本函数仅支持最优正交化的1、2、3阶级数解法
% # 本函数不能1次完全规范化，迭代调用可减少残差
%
% Input Arguments:
% # CHat: 3*3矩阵
% # order: 标量，阶次，缺省输入时采用1阶级数解法
%
% Output Arguments:
% # C0: 3*3矩阵，正交化后的方向余弦矩阵
%
% References:
% # 《应用导航算法工程基础》“方向余弦矩阵的正交化”

if nargin < 2
    order = 1;
end
ESYM = CHat*CHat' - eye(3);
assert(any(order==[1 2 3]))
% REF1 1.4.161式
tmpC = eye(3);
if order >= 1
    tmpC = tmpC - ESYM/2;
end
if order >= 2
    tmpC = tmpC + 3/8*ESYM^2;
end
if order >= 3
    tmpC = tmpC - 15/48*ESYM^3;
end
C0 = tmpC*CHat;
end